from ros2webserver.baseserver.capability import Capability
from sensor_msgs.msg import LaserScan  # 导入正确的雷达数据消息类型
import json

class LaserScanSubscriber(Capability):
    def __init__(self, protocol):
        super().__init__(protocol)  # 使用 super() 来调用超类的构造函数
        # 注册此能力提供的操作
        self.protocol.register_operation("fetchLaserScan", self.handle_fetch_laser_scan)

        # 订阅雷达数据话题 scan
        self.latest_laser_scan = None
        self.laser_scan_subscriber = self.protocol.node_handle.create_subscription(
            LaserScan,  # 消息类型
            'scan',  # 话题名称
            self.laser_scan_callback,  # 回调函数
            10  # 队列大小
        )

    def laser_scan_callback(self, msg):
        # 保存最新的雷达扫描数据，替换无穷大值为 0.0
        self.latest_laser_scan = msg
        if self.latest_laser_scan.ranges:
            self.latest_laser_scan.ranges = [
                0.0 if x == float('inf') else x for x in self.latest_laser_scan.ranges
            ]
    def handle_fetch_laser_scan(self, message):
        self.protocol.node_handle.get_logger().info(f"{message}")
        if self.latest_laser_scan:
            # 将雷达数据转换为JSON格式返回
            laser_data = {
                'ranges': list(self.latest_laser_scan.ranges),
                'intensities': list(self.latest_laser_scan.intensities),
                'angle_min': self.latest_laser_scan.angle_min,
                'angle_max': self.latest_laser_scan.angle_max,
                'angle_increment': self.latest_laser_scan.angle_increment,
                'time_increment': self.latest_laser_scan.time_increment,
                'scan_time': self.latest_laser_scan.scan_time,
                'range_min': self.latest_laser_scan.range_min,
                'range_max': self.latest_laser_scan.range_max,
            }
            self.protocol.send({
                "op": "fetchLaserScan",
                "code": 0,
                "msg": "ok",  
                "data": laser_data
            })
        else:
            self.protocol.send({
                "op": "fetchLaserScan",
                "code": 1,
                "msg": "failed",  
                "data": None
            })

    def finish(self):
        self.protocol.node_handle.destroy_subscription(self.laser_scan_subscriber)  # 修改为正确的函数名
